{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 动态窗口法"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 223,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import copy\n",
    "from celluloid import Camera  # 保存动图时用，pip install celluloid\n",
    "import math\n",
    "%matplotlib qt5\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 参数设置\n",
    "\n",
    "Reference: [AtsushiSakai/PythonRobotics ](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py#L34)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 224,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Config:\n",
    "    \"\"\"\n",
    "    simulation parameter class\n",
    "    \"\"\"\n",
    "\n",
    "    def __init__(self):\n",
    "        # robot parameter\n",
    "        # 线速度边界\n",
    "        self.v_max = 1.0  # [m/s]\n",
    "        self.v_min = -0.5  # [m/s]\n",
    "        # 角速度边界\n",
    "        self.w_max = 40.0 * math.pi / 180.0  # [rad/s]\n",
    "        self.w_min = -40.0 * math.pi / 180.0  # [rad/s]\n",
    "        # 线加速度和角加速度最大值\n",
    "        self.a_vmax = 0.2  # [m/ss]\n",
    "        self.a_wmax = 40.0 * math.pi / 180.0  # [rad/ss]\n",
    "        # 采样分辨率 \n",
    "        self.v_sample = 0.01  # [m/s]\n",
    "        self.w_sample = 0.1 * math.pi / 180.0  # [rad/s]\n",
    "        # 离散时间\n",
    "        self.dt = 0.1  # [s] Time tick for motion prediction\n",
    "        # 轨迹推算时间长度\n",
    "        self.predict_time = 3.0  # [s]\n",
    "        # 轨迹评价函数系数\n",
    "        self.alpha = 0.15\n",
    "        self.beta = 1.0\n",
    "        self.gamma = 1.0\n",
    "\n",
    "        # Also used to check if goal is reached in both types\n",
    "        self.robot_radius = 1.0  # [m] for collision check\n",
    "        \n",
    "        self.judge_distance = 10 # 若与障碍物的最小距离大于阈值（例如这里设置的阈值为robot_radius+0.2）,则设为一个较大的常值\n",
    "\n",
    "        # 障碍物位置 [x(m) y(m), ....]\n",
    "        self.ob = np.array([[-1, -1],\n",
    "                    [0, 2],\n",
    "                    [4.0, 2.0],\n",
    "                    [5.0, 4.0],\n",
    "                    [5.0, 5.0],\n",
    "                    [5.0, 6.0],\n",
    "                    [5.0, 9.0],\n",
    "                    [8.0, 9.0],\n",
    "                    [7.0, 9.0],\n",
    "                    [8.0, 10.0],\n",
    "                    [9.0, 11.0],\n",
    "                    [12.0, 13.0],\n",
    "                    [12.0, 12.0],\n",
    "                    [15.0, 15.0],\n",
    "                    [13.0, 13.0]\n",
    "                    ])\n",
    "        # 目标点位置\n",
    "        self.target = np.array([10,10])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 225,
   "metadata": {},
   "outputs": [],
   "source": [
    "def KinematicModel(state,control,dt):\n",
    "  \"\"\"机器人运动学模型\n",
    "\n",
    "  Args:\n",
    "      state (_type_): 状态量---x,y,yaw,v,w\n",
    "      control (_type_): 控制量---v,w,线速度和角速度\n",
    "      dt (_type_): 离散时间\n",
    "\n",
    "  Returns:\n",
    "      _type_: 下一步的状态\n",
    "  \"\"\"\n",
    "  state[0] += control[0] * math.cos(state[2]) * dt\n",
    "  state[1] += control[0] * math.sin(state[2]) * dt\n",
    "  state[2] += control[1] * dt\n",
    "  state[3] = control[0]\n",
    "  state[4] = control[1]\n",
    "\n",
    "  return state\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 226,
   "metadata": {},
   "outputs": [],
   "source": [
    "class DWA:\n",
    "    def __init__(self,config) -> None:\n",
    "        \"\"\"初始化\n",
    "\n",
    "        Args:\n",
    "            config (_type_): 参数类\n",
    "        \"\"\"\n",
    "        self.dt=config.dt\n",
    "        self.v_min=config.v_min\n",
    "        self.w_min=config.w_min\n",
    "        self.v_max=config.v_max\n",
    "        self.w_max=config.w_max\n",
    "        self.predict_time = config.predict_time\n",
    "        self.a_vmax = config.a_vmax\n",
    "        self.a_wmax = config.a_wmax\n",
    "        self.v_sample = config.v_sample # 线速度采样分辨率\n",
    "        self.w_sample = config.w_sample # 角速度采样分辨率\n",
    "        self.alpha = config.alpha\n",
    "        self.beta = config.beta\n",
    "        self.gamma = config.gamma\n",
    "        self.radius = config.robot_radius\n",
    "        self.judge_distance = config.judge_distance\n",
    "\n",
    "    def dwa_control(self,state,goal,obstacle):\n",
    "        \"\"\"滚动窗口算法入口\n",
    "\n",
    "        Args:\n",
    "            state (_type_): 机器人当前状态--[x,y,yaw,v,w]\n",
    "            goal (_type_): 目标点位置，[x,y]\n",
    "\n",
    "            obstacle (_type_): 障碍物位置，dim:[num_ob,2]\n",
    "\n",
    "        Returns:\n",
    "            _type_: 控制量、轨迹（便于绘画）\n",
    "        \"\"\"\n",
    "        control,trajectory = self.trajectory_evaluation(state,goal,obstacle)\n",
    "        return control,trajectory\n",
    "\n",
    "\n",
    "    def cal_dynamic_window_vel(self,v,w,state,obstacle):\n",
    "        \"\"\"速度采样,得到速度空间窗口\n",
    "\n",
    "        Args:\n",
    "            v (_type_): 当前时刻线速度\n",
    "            w (_type_): 当前时刻角速度\n",
    "            state (_type_): 当前机器人状态\n",
    "            obstacle (_type_): 障碍物位置\n",
    "        Returns:\n",
    "            [v_low,v_high,w_low,w_high]: 最终采样后的速度空间\n",
    "        \"\"\"\n",
    "        Vm = self.__cal_vel_limit()\n",
    "        Vd = self.__cal_accel_limit(v,w)\n",
    "        Va = self.__cal_obstacle_limit(state,obstacle)\n",
    "        a = max([Vm[0],Vd[0],Va[0]])\n",
    "        b = min([Vm[1],Vd[1],Va[1]])\n",
    "        c = max([Vm[2], Vd[2],Va[2]])\n",
    "        d = min([Vm[3], Vd[3],Va[3]])\n",
    "        return [a,b,c,d]\n",
    "\n",
    "    def __cal_vel_limit(self):\n",
    "        \"\"\"计算速度边界限制Vm\n",
    "\n",
    "        Returns:\n",
    "            _type_: 速度边界限制后的速度空间Vm\n",
    "        \"\"\"\n",
    "        return [self.v_min,self.v_max,self.w_min,self.w_max]\n",
    "    \n",
    "    def __cal_accel_limit(self,v,w):\n",
    "        \"\"\"计算加速度限制Vd\n",
    "\n",
    "        Args:\n",
    "            v (_type_): 当前时刻线速度\n",
    "            w (_type_): 当前时刻角速度\n",
    "        Returns: \n",
    "            _type_:考虑加速度时的速度空间Vd\n",
    "        \"\"\"\n",
    "        v_low = v-self.a_vmax*self.dt\n",
    "        v_high = v+self.a_vmax*self.dt\n",
    "        w_low = w-self.a_wmax*self.dt\n",
    "        w_high = w+self.a_wmax*self.dt\n",
    "        return [v_low, v_high,w_low, w_high]\n",
    "    \n",
    "    def __cal_obstacle_limit(self,state,obstacle):\n",
    "        \"\"\"环境障碍物限制Va\n",
    "\n",
    "        Args:\n",
    "            state (_type_): 当前机器人状态\n",
    "            obstacle (_type_): 障碍物位置\n",
    "\n",
    "        Returns:\n",
    "            _type_: 某一时刻移动机器人不与周围障碍物发生碰撞的速度空间Va\n",
    "        \"\"\"\n",
    "        v_low=self.v_min\n",
    "        v_high = np.sqrt(2*self._dist(state,obstacle)*self.a_vmax)\n",
    "        w_low = self.w_min\n",
    "        w_high = np.sqrt(2*self._dist(state,obstacle)*self.a_wmax)\n",
    "        return [v_low,v_high,w_low,w_high]\n",
    "\n",
    "    def trajectory_predict(self,state_init, v,w):\n",
    "        \"\"\"轨迹推算\n",
    "\n",
    "        Args:\n",
    "            state_init (_type_): 当前状态---x,y,yaw,v,w\n",
    "            v (_type_): 当前时刻线速度\n",
    "            w (_type_): 当前时刻线速度\n",
    "\n",
    "        Returns:\n",
    "            _type_: _description_\n",
    "        \"\"\"\n",
    "        state = np.array(state_init)\n",
    "        trajectory = state\n",
    "        time = 0\n",
    "        # 在预测时间段内\n",
    "        while time <= self.predict_time:\n",
    "            x = KinematicModel(state, [v,w], self.dt) # 运动学模型\n",
    "            trajectory = np.vstack((trajectory, x))\n",
    "            time += self.dt\n",
    "\n",
    "        return trajectory\n",
    "\n",
    "    def trajectory_evaluation(self,state,goal,obstacle):\n",
    "        \"\"\"轨迹评价函数,评价越高，轨迹越优\n",
    "\n",
    "        Args:\n",
    "            state (_type_): 当前状态---x,y,yaw,v,w\n",
    "            dynamic_window_vel (_type_): 采样的速度空间窗口---[v_low,v_high,w_low,w_high]\n",
    "            goal (_type_): 目标点位置，[x,y]\n",
    "            obstacle (_type_): 障碍物位置，dim:[num_ob,2]\n",
    "\n",
    "        Returns:\n",
    "            _type_: 最优控制量、最优轨迹\n",
    "        \"\"\"\n",
    "        G_max = -float('inf') # 最优评价\n",
    "        trajectory_opt = state # 最优轨迹\n",
    "        control_opt = [0.,0.] # 最优控制\n",
    "        dynamic_window_vel = self.cal_dynamic_window_vel(state[3], state[4],state,obstacle) # 第1步--计算速度空间\n",
    "        \n",
    "        # sum_heading,sum_dist,sum_vel = 0,0,0 # 统计全部采样轨迹的各个评价之和，便于评价的归一化\n",
    "        # # 在本次实验中，不进行归一化也可实现该有的效果。\n",
    "        # for v in np.arange(dynamic_window_vel[0],dynamic_window_vel[1],self.v_sample):\n",
    "        #     for w in np.arange(dynamic_window_vel[2], dynamic_window_vel[3], self.w_sample):   \n",
    "        #         trajectory = self.trajectory_predict(state, v, w)  \n",
    "\n",
    "        #         heading_eval = self.alpha*self.__heading(trajectory,goal)\n",
    "        #         dist_eval = self.beta*self.__dist(trajectory,obstacle)\n",
    "        #         vel_eval = self.gamma*self.__velocity(trajectory)\n",
    "        #         sum_vel+=vel_eval\n",
    "        #         sum_dist+=dist_eval\n",
    "        #         sum_heading +=heading_eval\n",
    "\n",
    "        # 在速度空间中按照预先设定的分辨率采样\n",
    "        sum_heading,sum_dist,sum_vel = 1,1,1 # 不进行归一化\n",
    "        for v in np.arange(dynamic_window_vel[0],dynamic_window_vel[1],self.v_sample):\n",
    "            for w in np.arange(dynamic_window_vel[2], dynamic_window_vel[3], self.w_sample):\n",
    "\n",
    "                trajectory = self.trajectory_predict(state, v, w)  # 第2步--轨迹推算\n",
    "\n",
    "                heading_eval = self.alpha*self.__heading(trajectory,goal)/sum_heading\n",
    "                dist_eval = self.beta*self.__dist(trajectory,obstacle)/sum_dist\n",
    "                vel_eval = self.gamma*self.__velocity(trajectory)/sum_vel\n",
    "                G = heading_eval+dist_eval+vel_eval # 第3步--轨迹评价\n",
    "\n",
    "                if G_max<=G:\n",
    "                    G_max = G\n",
    "                    trajectory_opt = trajectory\n",
    "                    control_opt = [v,w]\n",
    "\n",
    "        return control_opt, trajectory_opt\n",
    "\n",
    "    def _dist(self,state,obstacle):\n",
    "        \"\"\"计算当前移动机器人距离障碍物最近的几何距离\n",
    "\n",
    "        Args:\n",
    "            state (_type_): 当前机器人状态\n",
    "            obstacle (_type_): 障碍物位置\n",
    "\n",
    "        Returns:\n",
    "            _type_: 移动机器人距离障碍物最近的几何距离\n",
    "        \"\"\"\n",
    "        ox = obstacle[:, 0]\n",
    "        oy = obstacle[:, 1]\n",
    "        dx = state[0,None] - ox[:, None]\n",
    "        dy = state[1,None] - oy[:, None]\n",
    "        r = np.hypot(dx, dy)\n",
    "        return np.min(r)\n",
    "\n",
    "    def __dist(self,trajectory,obstacle):\n",
    "        \"\"\"距离评价函数\n",
    "        表示当前速度下对应模拟轨迹与障碍物之间的最近距离；\n",
    "        如果没有障碍物或者最近距离大于设定的阈值，那么就将其值设为一个较大的常数值。\n",
    "        Args:\n",
    "            trajectory (_type_): 轨迹，dim:[n,5]\n",
    "            \n",
    "            obstacle (_type_): 障碍物位置，dim:[num_ob,2]\n",
    "\n",
    "        Returns:\n",
    "            _type_: _description_\n",
    "        \"\"\"\n",
    "        ox = obstacle[:, 0]\n",
    "        oy = obstacle[:, 1]\n",
    "        dx = trajectory[:, 0] - ox[:, None]\n",
    "        dy = trajectory[:, 1] - oy[:, None]\n",
    "        r = np.hypot(dx, dy)\n",
    "        return np.min(r) if np.array(r <self.radius+0.2).any() else self.judge_distance\n",
    "\n",
    "    def __heading(self,trajectory, goal):\n",
    "        \"\"\"方位角评价函数\n",
    "        评估在当前采样速度下产生的轨迹终点位置方向与目标点连线的夹角的误差\n",
    "\n",
    "        Args:\n",
    "            trajectory (_type_): 轨迹，dim:[n,5]\n",
    "            goal (_type_): 目标点位置[x,y]\n",
    "\n",
    "        Returns:\n",
    "            _type_: 方位角评价数值\n",
    "        \"\"\"\n",
    "        dx = goal[0] - trajectory[-1, 0]\n",
    "        dy = goal[1] - trajectory[-1, 1]\n",
    "        error_angle = math.atan2(dy, dx)\n",
    "        cost_angle = error_angle - trajectory[-1, 2]\n",
    "        cost = math.pi-abs(cost_angle)\n",
    "\n",
    "        return cost\n",
    "\n",
    "    def __velocity(self,trajectory):\n",
    "        \"\"\"速度评价函数， 表示当前的速度大小，可以用模拟轨迹末端位置的线速度的大小来表示\n",
    "\n",
    "        Args:\n",
    "            trajectory (_type_): 轨迹，dim:[n,5]\n",
    "\n",
    "        Returns:\n",
    "            _type_: 速度评价\n",
    "        \"\"\"\n",
    "        return trajectory[-1,3]\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 画图\n",
    "\n",
    "Reference: [AtsushiSakai/PythonRobotics ](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py#L231)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 227,
   "metadata": {},
   "outputs": [],
   "source": [
    "def plot_arrow(x, y, yaw, length=0.5, width=0.1):  # pragma: no cover\n",
    "    plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),\n",
    "              head_length=width, head_width=width)\n",
    "    plt.plot(x, y)\n",
    "\n",
    "\n",
    "def plot_robot(x, y, yaw, config):  # pragma: no cover\n",
    "        circle = plt.Circle((x, y), config.robot_radius, color=\"b\")\n",
    "        plt.gcf().gca().add_artist(circle)\n",
    "        out_x, out_y = (np.array([x, y]) +\n",
    "                        np.array([np.cos(yaw), np.sin(yaw)]) * config.robot_radius)\n",
    "        plt.plot([x, out_x], [y, out_y], \"-k\")\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### 主函数\n",
    "\n",
    "Reference: [AtsushiSakai/PythonRobotics ](https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py#L260)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 228,
   "metadata": {},
   "outputs": [
    {
     "ename": "KeyboardInterrupt",
     "evalue": "",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
      "\u001b[1;32me:\\CHH3213_KING\\研究生\\导师\\就业规划\\自动驾驶\\chhRobotics\\PathPlanning\\动态窗口法(DWA)\\DynamicWindowApproaches.ipynb Cell 10\u001b[0m in \u001b[0;36m<cell line: 57>\u001b[1;34m()\u001b[0m\n\u001b[0;32m     <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=50'>51</a>\u001b[0m     \u001b[39m# camera.snap()\u001b[39;00m\n\u001b[0;32m     <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=51'>52</a>\u001b[0m     \u001b[39m# animation = camera.animate()\u001b[39;00m\n\u001b[0;32m     <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=52'>53</a>\u001b[0m     \u001b[39m# animation.save('trajectory.gif')\u001b[39;00m\n\u001b[0;32m     <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=53'>54</a>\u001b[0m     plt\u001b[39m.\u001b[39mshow()\n\u001b[1;32m---> <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=56'>57</a>\u001b[0m main(Config())\n",
      "\u001b[1;32me:\\CHH3213_KING\\研究生\\导师\\就业规划\\自动驾驶\\chhRobotics\\PathPlanning\\动态窗口法(DWA)\\DynamicWindowApproaches.ipynb Cell 10\u001b[0m in \u001b[0;36mmain\u001b[1;34m(config)\u001b[0m\n\u001b[0;32m     <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=16'>17</a>\u001b[0m x \u001b[39m=\u001b[39m KinematicModel(x, u, config\u001b[39m.\u001b[39mdt)  \u001b[39m# simulate robot\u001b[39;00m\n\u001b[0;32m     <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=17'>18</a>\u001b[0m trajectory \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39mvstack((trajectory, x))  \u001b[39m# store state history\u001b[39;00m\n\u001b[1;32m---> <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=18'>19</a>\u001b[0m plt\u001b[39m.\u001b[39;49mcla()\n\u001b[0;32m     <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=19'>20</a>\u001b[0m \u001b[39m# for stopping simulation with the esc key.\u001b[39;00m\n\u001b[0;32m     <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=20'>21</a>\u001b[0m plt\u001b[39m.\u001b[39mgcf()\u001b[39m.\u001b[39mcanvas\u001b[39m.\u001b[39mmpl_connect(\n\u001b[0;32m     <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=21'>22</a>\u001b[0m     \u001b[39m'\u001b[39m\u001b[39mkey_release_event\u001b[39m\u001b[39m'\u001b[39m,\n\u001b[0;32m     <a href='vscode-notebook-cell:/e%3A/CHH3213_KING/%E7%A0%94%E7%A9%B6%E7%94%9F/%E5%AF%BC%E5%B8%88/%E5%B0%B1%E4%B8%9A%E8%A7%84%E5%88%92/%E8%87%AA%E5%8A%A8%E9%A9%BE%E9%A9%B6/chhRobotics/PathPlanning/%E5%8A%A8%E6%80%81%E7%AA%97%E5%8F%A3%E6%B3%95%28DWA%29/DynamicWindowApproaches.ipynb#X36sZmlsZQ%3D%3D?line=22'>23</a>\u001b[0m     \u001b[39mlambda\u001b[39;00m event: [exit(\u001b[39m0\u001b[39m) \u001b[39mif\u001b[39;00m event\u001b[39m.\u001b[39mkey \u001b[39m==\u001b[39m \u001b[39m'\u001b[39m\u001b[39mescape\u001b[39m\u001b[39m'\u001b[39m \u001b[39melse\u001b[39;00m \u001b[39mNone\u001b[39;00m])\n",
      "File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\pyplot.py:1113\u001b[0m, in \u001b[0;36mcla\u001b[1;34m()\u001b[0m\n\u001b[0;32m   1111\u001b[0m \u001b[39m\"\"\"Clear the current axes.\"\"\"\u001b[39;00m\n\u001b[0;32m   1112\u001b[0m \u001b[39m# Not generated via boilerplate.py to allow a different docstring.\u001b[39;00m\n\u001b[1;32m-> 1113\u001b[0m \u001b[39mreturn\u001b[39;00m gca()\u001b[39m.\u001b[39;49mcla()\n",
      "File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\axes\\_base.py:1301\u001b[0m, in \u001b[0;36m_AxesBase.cla\u001b[1;34m(self)\u001b[0m\n\u001b[0;32m   1297\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mpatch\u001b[39m.\u001b[39mset_transform(\u001b[39mself\u001b[39m\u001b[39m.\u001b[39mtransAxes)\n\u001b[0;32m   1299\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mset_axis_on()\n\u001b[1;32m-> 1301\u001b[0m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mxaxis\u001b[39m.\u001b[39;49mset_clip_path(\u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mpatch)\n\u001b[0;32m   1302\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39myaxis\u001b[39m.\u001b[39mset_clip_path(\u001b[39mself\u001b[39m\u001b[39m.\u001b[39mpatch)\n\u001b[0;32m   1304\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_shared_axes[\u001b[39m\"\u001b[39m\u001b[39mx\u001b[39m\u001b[39m\"\u001b[39m]\u001b[39m.\u001b[39mclean()\n",
      "File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\axis.py:935\u001b[0m, in \u001b[0;36mAxis.set_clip_path\u001b[1;34m(self, clippath, transform)\u001b[0m\n\u001b[0;32m    934\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mset_clip_path\u001b[39m(\u001b[39mself\u001b[39m, clippath, transform\u001b[39m=\u001b[39m\u001b[39mNone\u001b[39;00m):\n\u001b[1;32m--> 935\u001b[0m     \u001b[39msuper\u001b[39;49m()\u001b[39m.\u001b[39;49mset_clip_path(clippath, transform)\n\u001b[0;32m    936\u001b[0m     \u001b[39mfor\u001b[39;00m child \u001b[39min\u001b[39;00m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mmajorTicks \u001b[39m+\u001b[39m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mminorTicks:\n\u001b[0;32m    937\u001b[0m         child\u001b[39m.\u001b[39mset_clip_path(clippath, transform)\n",
      "File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\artist.py:790\u001b[0m, in \u001b[0;36mArtist.set_clip_path\u001b[1;34m(self, path, transform)\u001b[0m\n\u001b[0;32m    787\u001b[0m \u001b[39mif\u001b[39;00m transform \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m:\n\u001b[0;32m    788\u001b[0m     \u001b[39mif\u001b[39;00m \u001b[39misinstance\u001b[39m(path, Rectangle):\n\u001b[0;32m    789\u001b[0m         \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mclipbox \u001b[39m=\u001b[39m TransformedBbox(Bbox\u001b[39m.\u001b[39munit(),\n\u001b[1;32m--> 790\u001b[0m                                        path\u001b[39m.\u001b[39;49mget_transform())\n\u001b[0;32m    791\u001b[0m         \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_clippath \u001b[39m=\u001b[39m \u001b[39mNone\u001b[39;00m\n\u001b[0;32m    792\u001b[0m         success \u001b[39m=\u001b[39m \u001b[39mTrue\u001b[39;00m\n",
      "File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\patches.py:278\u001b[0m, in \u001b[0;36mPatch.get_transform\u001b[1;34m(self)\u001b[0m\n\u001b[0;32m    276\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mget_transform\u001b[39m(\u001b[39mself\u001b[39m):\n\u001b[0;32m    277\u001b[0m     \u001b[39m\"\"\"Return the `~.transforms.Transform` applied to the `Patch`.\"\"\"\u001b[39;00m\n\u001b[1;32m--> 278\u001b[0m     \u001b[39mreturn\u001b[39;00m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mget_patch_transform() \u001b[39m+\u001b[39m artist\u001b[39m.\u001b[39mArtist\u001b[39m.\u001b[39mget_transform(\u001b[39mself\u001b[39m)\n",
      "File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\patches.py:760\u001b[0m, in \u001b[0;36mRectangle.get_patch_transform\u001b[1;34m(self)\u001b[0m\n\u001b[0;32m    753\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mget_patch_transform\u001b[39m(\u001b[39mself\u001b[39m):\n\u001b[0;32m    754\u001b[0m     \u001b[39m# Note: This cannot be called until after this has been added to\u001b[39;00m\n\u001b[0;32m    755\u001b[0m     \u001b[39m# an Axes, otherwise unit conversion will fail. This makes it very\u001b[39;00m\n\u001b[0;32m    756\u001b[0m     \u001b[39m# important to call the accessor method and not directly access the\u001b[39;00m\n\u001b[0;32m    757\u001b[0m     \u001b[39m# transformation member variable.\u001b[39;00m\n\u001b[0;32m    758\u001b[0m     bbox \u001b[39m=\u001b[39m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mget_bbox()\n\u001b[0;32m    759\u001b[0m     \u001b[39mreturn\u001b[39;00m (transforms\u001b[39m.\u001b[39mBboxTransformTo(bbox)\n\u001b[1;32m--> 760\u001b[0m             \u001b[39m+\u001b[39m transforms\u001b[39m.\u001b[39;49mAffine2D()\u001b[39m.\u001b[39;49mrotate_deg_around(\n\u001b[0;32m    761\u001b[0m                 bbox\u001b[39m.\u001b[39;49mx0, bbox\u001b[39m.\u001b[39;49my0, \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mangle))\n",
      "File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\transforms.py:2042\u001b[0m, in \u001b[0;36mAffine2D.rotate_deg_around\u001b[1;34m(self, x, y, degrees)\u001b[0m\n\u001b[0;32m   2040\u001b[0m \u001b[39m# Cast to float to avoid wraparound issues with uint8's\u001b[39;00m\n\u001b[0;32m   2041\u001b[0m x, y \u001b[39m=\u001b[39m \u001b[39mfloat\u001b[39m(x), \u001b[39mfloat\u001b[39m(y)\n\u001b[1;32m-> 2042\u001b[0m \u001b[39mreturn\u001b[39;00m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mtranslate(\u001b[39m-\u001b[39;49mx, \u001b[39m-\u001b[39;49my)\u001b[39m.\u001b[39;49mrotate_deg(degrees)\u001b[39m.\u001b[39mtranslate(x, y)\n",
      "File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\transforms.py:2020\u001b[0m, in \u001b[0;36mAffine2D.rotate_deg\u001b[1;34m(self, degrees)\u001b[0m\n\u001b[0;32m   2012\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mrotate_deg\u001b[39m(\u001b[39mself\u001b[39m, degrees):\n\u001b[0;32m   2013\u001b[0m     \u001b[39m\"\"\"\u001b[39;00m\n\u001b[0;32m   2014\u001b[0m \u001b[39m    Add a rotation (in degrees) to this transform in place.\u001b[39;00m\n\u001b[0;32m   2015\u001b[0m \n\u001b[1;32m   (...)\u001b[0m\n\u001b[0;32m   2018\u001b[0m \u001b[39m    and :meth:`scale`.\u001b[39;00m\n\u001b[0;32m   2019\u001b[0m \u001b[39m    \"\"\"\u001b[39;00m\n\u001b[1;32m-> 2020\u001b[0m     \u001b[39mreturn\u001b[39;00m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mrotate(math\u001b[39m.\u001b[39;49mradians(degrees))\n",
      "File \u001b[1;32md:\\ProgramData\\Anaconda3\\envs\\gobigger\\lib\\site-packages\\matplotlib\\transforms.py:2008\u001b[0m, in \u001b[0;36mAffine2D.rotate\u001b[1;34m(self, theta)\u001b[0m\n\u001b[0;32m   2005\u001b[0m b \u001b[39m=\u001b[39m math\u001b[39m.\u001b[39msin(theta)\n\u001b[0;32m   2006\u001b[0m rotate_mtx \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39marray([[a, \u001b[39m-\u001b[39mb, \u001b[39m0.0\u001b[39m], [b, a, \u001b[39m0.0\u001b[39m], [\u001b[39m0.0\u001b[39m, \u001b[39m0.0\u001b[39m, \u001b[39m1.0\u001b[39m]],\n\u001b[0;32m   2007\u001b[0m                       \u001b[39mfloat\u001b[39m)\n\u001b[1;32m-> 2008\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39m_mtx \u001b[39m=\u001b[39m np\u001b[39m.\u001b[39;49mdot(rotate_mtx, \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49m_mtx)\n\u001b[0;32m   2009\u001b[0m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39minvalidate()\n\u001b[0;32m   2010\u001b[0m \u001b[39mreturn\u001b[39;00m \u001b[39mself\u001b[39m\n",
      "File \u001b[1;32m<__array_function__ internals>:180\u001b[0m, in \u001b[0;36mdot\u001b[1;34m(*args, **kwargs)\u001b[0m\n",
      "\u001b[1;31mKeyboardInterrupt\u001b[0m: "
     ]
    }
   ],
   "source": [
    "def main(config):\n",
    "    # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]\n",
    "    x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])\n",
    "    # goal position [x(m), y(m)]\n",
    "    goal = config.target\n",
    "\n",
    "    # input [forward speed, yaw_rate]\n",
    "\n",
    "    trajectory = np.array(x)\n",
    "    ob = config.ob\n",
    "    dwa = DWA(config)\n",
    "    fig=plt.figure(1)\n",
    "    camera = Camera(fig)\n",
    "    while True:\n",
    "        u, predicted_trajectory = dwa.dwa_control(x,goal, ob)\n",
    "\n",
    "        x = KinematicModel(x, u, config.dt)  # simulate robot\n",
    "        trajectory = np.vstack((trajectory, x))  # store state history\n",
    "        plt.cla()\n",
    "        # for stopping simulation with the esc key.\n",
    "        plt.gcf().canvas.mpl_connect(\n",
    "            'key_release_event',\n",
    "            lambda event: [exit(0) if event.key == 'escape' else None])\n",
    "        plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], \"-g\")\n",
    "        plt.plot(x[0], x[1], \"xr\")\n",
    "        plt.plot(goal[0], goal[1], \"xb\")\n",
    "        plt.plot(ob[:, 0], ob[:, 1], \"ok\")\n",
    "        plot_robot(x[0], x[1], x[2], config)\n",
    "        plot_arrow(x[0], x[1], x[2])\n",
    "        \n",
    "        formatted_state = [f\"{value:.3f}\" for value in x]\n",
    "        annotation_text = f\"[x, y, yaw, v, w]:\\n{formatted_state}\"\n",
    "        plt.annotate(annotation_text, (x[0], x[1]), textcoords=\"offset points\", xytext=(0, 10), ha='center')\n",
    "    \n",
    "        plt.axis(\"equal\")\n",
    "        plt.grid(True)\n",
    "        plt.pause(0.001)\n",
    "\n",
    "        # check reaching goal\n",
    "        dist_to_goal = math.hypot(x[0] - goal[0], x[1] - goal[1])\n",
    "        if dist_to_goal <= config.robot_radius:\n",
    "            print(\"Goal!!\")\n",
    "            break\n",
    "        # camera.snap()\n",
    "        # print(x)\n",
    "        # print(u)\n",
    "\n",
    "    print(\"Done\")\n",
    "    plt.plot(trajectory[:, 0], trajectory[:, 1], \"-r\")\n",
    "    plt.pause(0.001)\n",
    "    # camera.snap()\n",
    "    # animation = camera.animate()\n",
    "    # animation.save('trajectory.gif')\n",
    "    plt.show()\n",
    "\n",
    "\n",
    "main(Config())\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.12 ('gobigger')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.16"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "0c7484b3574347463e16b31029466871583b0d4e5c4ad861e8848f2d3746b4de"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
